from copy import copy
import pygame
import math
import UAI

def getAngleAndDistanceOfTwoPoints(p1, p2):
    p1x, p1y = p1
    p2x, p2y = p2
    diffx    = p1x - p2x
    diffy    = p1y - p2y
    distance = math.sqrt(math.pow(diffy, 2) + math.pow(diffx, 2))
    angle    = math.pi + math.atan2(diffy, diffx)
    return math.degrees(angle), distance

def getDistance(p1, p2):
    p1x, p1y = p1
    p2x, p2y = p2
    diffx    = p1x - p2x
    diffy    = p1y - p2y
    distance = math.sqrt(math.pow(diffy, 2) + math.pow(diffx, 2))
    return distance

def getPointOfAngleAndDistance(angle, distance):
    angle = math.radians(angle)
    return distance * math.cos(angle), distance * math.sin(angle)

def collideOnLine(rect1, p1, p2, limit=100):
    angle, distance = getAngleAndDistanceOfTwoPoints(p1, p2)
    angle = math.radians(angle)
    x = p1[0]
    y = p1[1]
    i = 0
    while i < limit:
        diffx = math.cos(angle)
        diffy = math.sin(angle)
        x = x + diffx
        y = y + diffy
        if rect1.collidepoint(x,y):
            return True
        i = i + 1
    return False

def collideRectOnLine(rect1, rect2, p1, p2, limit=100):
    angle, distance = getAngleAndDistanceOfTwoPoints(p1, p2)
    angle = math.radians(angle)
    testrect = copy(rect2)
    x = p1[0]
    y = p1[1]
    i = 0
    xt = x - p2[0]
    xtest = False
    ytest = False
    if xt > 0:
        xtest = True
    yt = y - p2[1]
    if yt > 0:
        ytest = True
    while i < limit:
        xt = x - p2[0]
        if xtest:
            if xt < 0:
                return False
        else:
            if xt > 0:
                return False
        yt = y - p2[1]
        if ytest:
            if yt < 0:
                return False
        else:
            if yt > 0:
                return False
            
        diffx = math.cos(angle)
        diffy = math.sin(angle)
        x = x + diffx
        y = y + diffy
        testrect.inflate(1,1)
        testrect.center = (x,y)
        if rect1.colliderect(testrect):
            return True
        i = i + 1
    return False

def normalize(i):
    if i < 0:
        return 0 - i
    return i